/**************************************************************************************************

	@file   	table_lookup_jz.c
	@author 	Jon Zeeff
	@date   	September, 2011
	@brief  	table lookup - fast, generic, 1D, 3D, variable axis, fixed axis, int16_t or uint8_t, binary search 
	@copyright 	MIT license
	@warning 	needs more testing - send results to jon@zeeff.com
	@version 	1.3
	
	@note Generic, portable 1D or 2D table lookup
	Can use a fixed interval on the axis (tends to need more points to get accuracy but is faster)
	Table entries are int16_t (which will usually work with unsigned values too) or 8 bit unsigned
	Works with any bin point
	Uses a binary search when doing variable axis increments (faster)
	Linear search is available if you have small tables
	Logs errors if desired
	Does range and error checks
	ISO C90 compatible (yuck, check with "gcc -Wall -pedantic -Wextra")
	
	To use this, you need to fill out a table_jz structure and then pass the lookup 
	routine a pointer to the structure.
	
****************************************************************************************************/

/* 

Copyright (c) 2011 Jon Zeeff

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

*/

#include <stdint.h>
#include "Table_Lookup_JZ.h"

/* Use this if you don't have error logging */
#define system_error(a,b,c,d)   /* a routine to log a string */
/*  #define MAIN if you want to test this as a standalone program on a PC */
/* #define MAIN */
/*  #define TEST if you want some printout */
/* #define TEST */
#ifndef FALSE
#define FALSE 0
#define TRUE 1
#endif

#ifdef MAIN
#include <stdio.h>
#include <memory.h>

/* example of how to define a table in rom or flash */

const struct table_3x3_16 table1 = {
    .rows = 3,
    .cols = 3,
    .variable_axis = FALSE,     /* default is zero anyway */
    .byte_table = FALSE,        /* default is zero anyway */
    .min_x = 0,
    .max_x = 1000,
    .min_y = 0,
    .max_y = 100,
    /* data, a full row on each line: */
    .data = {
             {1, 2, 3},
             {4, 5, 6},
             {7, 8, 9}}
};

int main()
{
    /* Example of use */
    int32_t j;

    printf("\n %d %d %d \n %d %d %d \n %d %d %d\n",
           table1.data[0][0], table1.data[0][1], table1.data[0][2],
           table1.data[1][0], table1.data[1][1], table1.data[1][2],
           table1.data[2][0], table1.data[2][1], table1.data[2][2]
        );

    fflush(stdout);

    printf("x axis ranges from %d to %d\n", table1.min_x, table1.max_x);
    printf("y axis ranges from %d to %d\n", table1.min_y, table1.max_y);
    printf("note: the origin is the upper left\n");

    /*  for (i = 0; i < 100000000; ++i)  // for speed test */
    j = table_lookup_jz(495, 45, (struct table_jz *)&table1);
    printf("f(x,y) = %d\n", j);

#if 0
#ifdef TEST
    i = table_lookup_jz(460, 460, &table);
    printf("f(x,y) = %d\n", i);
    i = table_lookup_jz(1000, 1000, &table);
    printf("f(x,y) = %d\n", i);
    i = table_lookup_jz(0, 0, &table);
    printf("f(x,y) = %d\n", i);
    i = table_lookup_jz(500, 500, &table);
    printf("f(x,y) = %d\n", i);
    i = table_lookup_jz(0, 1000, &table);
    printf("f(x,y) = %d\n", i);
    i = table_lookup_jz(1000, 0, &table);
    printf("f(x,y) = %d\n", i);
    i = table_lookup_jz(2000, 2000, &table);
    printf("f(x,y) = %d\n", i);
    i = table_lookup_jz(1000, -5, &table);
    printf("f(x,y) = %d\n", i);
#endif
#endif

    return 1;
}                               /* main() */
#endif


#if 0
/* given a value, n and a sorted array of n int16_t elements, return the index to the entry that is less than or equal to value */

inline unsigned
lsearch_jz(register const int value, const int16_t * array, unsigned n)
{
    register int16_t *ptr = (int16_t *) array + --n;    /* move to last position in array */

#ifdef TEST
    printf("lsearch %d %d %d for %d\n", array[0], array[1], array[2], value);
#endif

    /* out of bounds checks */
    if (value <= *array)
        return 0;

    /* check for too big */
    if (value >= *ptr)
        return n;

    while (*ptr > value)
        --ptr;                  /* loop from large to small */

    return (unsigned)(ptr - array);
}
#endif

/* generic binary search for a int16_t array */

inline unsigned
bsearch_jz(register const int value, const int16_t * array, unsigned n)
{
    register unsigned lower;
    register unsigned middle;
    register unsigned upper;

#ifdef TEST
    printf("bsearch %d %d %d for %d\n", array[0], array[1], array[2], value);
#endif

    /* out of bounds checks */
    if (value <= *array)        /* too low */
        return 0;

    upper = --n;                /* note, n is now max index */
    if (value >= array[upper])  /* too high */
        return n;

    lower = 0;

    for (;;) {
        middle = (upper - lower) >> 1;  /* 1/2 way between them */

        if (middle == lower)
            return lower;       /* done */

        if (array[middle] < value)
            lower = middle;
        else if (array[middle] > value)
            upper = middle;
        else                    /* equal */
            return middle;
    }
    return 0;                   /* never get here */
}

#ifdef BSEARCH_TEST

#include <stdio.h>

int main()
{
    int16_t array[6] = { 1, 5, 10, 15, 20, 29 };

    printf("array = 1, 5, 10, 15, 20, 29\n");
    printf("6 = %d\n", bsearch(6, array, 6));
    printf("0 = %d\n", bsearch(0, array, 6));
    printf("30 = %d\n", bsearch(30, array, 6));
    printf("1 = %d\n", bsearch(1, array, 6));
    printf("20 = %d\n", bsearch(20, array, 6));
    printf("17 = %d\n", bsearch(17, array, 6));

    return 0;
}

#endif
/* Given a fraction, find value between two values */

inline static int
interpolate(const unsigned fraction, const int value1, const int value2)
/* Note: fraction is bin 8 and ranges 0 to 1 */
{
#ifdef TEST
    printf("interpolate %f between %d and %d\n", fraction / 256., value1,
           value2);
#endif
    if (value1 == value2 || fraction == 0)      /* for speed */
        return value1;

    if (value1 < value2)        /* postitive slope */
        return value1 + (((value2 - value1) * (int32_t) fraction) >> 8);        /* correct back to bin 0  */
    else
        return value1 - (((value1 - value2) * (int32_t) fraction) >> 8);        /* correct back to bin 0  */
}

/*  macro to extract value from table - could be u8 or s16 */
#define value(index)   (table->byte_table !=0 ? (int16_t)(*(uint8_t *)(index)) : (*(int16_t *)(index)))

/************************************************************************

@param x value
@param y value (optional)
@param pointer to table structure
@return lookup value from table

************************************************************************/
int table_lookup_jz(const int x, const int y, const struct table_jz *table)
{
    register unsigned xbin;     /* bin 8, the x value converted to an index with fraction */
    register unsigned ybin;     /* bin 8, the y value converted to an index with fraction */
    register unsigned x_index;  /* bin 0 index version of xbin */
    register unsigned y_index;  /* bin 0 index */
    register uint8_t *ptr;      /* this pointer actually points to uint8s or int16s */
    register int value1;
    register unsigned entry_size;       /* 8 bit or 16 bit lookup */

    entry_size = table->byte_table != 0 ? 0 : 1;        /* 0 = 8 bit, 1 = 16 bit, later used as 2**entry_table */

#ifdef TEST
    printf("\ntable lookup of %d, %d\n", x, y);
#endif

    if (table == 0 || table->rows < 1 || table->cols < 2 || table->rows > 200) {     /* error check */
        system_error(3879, __FILE__, __LINE__, "");
        return 0;
    }

    /* find fractional indexes that point into the table */

    if (table->variable_axis == 0) {    /* use fixed axis increment method */
        /* calc x index by offsetting and scaling */
        if (x <= table->min_x)  /* less than min  */
            x_index = xbin = 0;
        else if (x >= table->max_x) {   /* greater than max */
            x_index = (unsigned)(table->cols - 1);
            xbin = 0;
        } else {                /* in the middle, calc how far */
            xbin = ((table->cols - 1) * (uint32_t) (x - table->min_x) << 8) / (table->max_x - table->min_x);    /* result is bin 8 */
            x_index = (xbin >> 8);      /* convert factional index to integer index */
            xbin &= 0xff;
        }

        /* calc y index by offsetting and scaling */
        if (table->rows == 1 || y <= table->min_y) {
            y_index = ybin = 0;
            ptr = (uint8_t *) table->data + (x_index << entry_size);
        } else {                /* find y position in table  */
            if (y >= table->max_y) {
                y_index = (unsigned)(table->rows - 1);
                ybin = 0;
            } else {            /* in the middle */
                ybin = ((table->rows - 1) * (uint32_t) (y - table->min_y) << 8) / (table->max_y - table->min_y);        /* result is bin 8 */
                y_index = (ybin >> 8);
                y_index &= 0xff;
            }
            ptr =
                (uint8_t *) table->data +
                (((table->cols * y_index) + x_index) << entry_size);
        }
    } else {                    /* variable axis */

        /* find fractional indexes using variable axis increments method */
        x_index = bsearch_jz(x, table->x_axis, table->cols);

        xbin = (unsigned)((x - table->x_axis[x_index]) << 8) / (table->x_axis[x_index + 1] - table->x_axis[x_index]);   /* bin 8 result */

        /* do y axis if this is a 2D lookup */

        if (table->rows == 1 || y <= table->min_y) {    /* 1D or outside of table */
            y_index = ybin = 0;
            ptr = (uint8_t *) table->data + (x_index << entry_size);
        } else {                /* find y position in table */
            y_index = bsearch_jz(y, table->y_axis, table->rows);
            ybin =
                (unsigned)((y -
                            table->y_axis[y_index]) << 8) /
                (table->y_axis[y_index + 1] - table->y_axis[y_index]);
            ptr =
                (uint8_t *) table->data +
                (((table->cols * y_index) + x_index) << entry_size);
        }
    }

    /* now interpolate from current point to next point for accuracy */

#ifdef TEST
    printf("variable axis  = %d\n", table->variable_axis);
    printf("x index = %f\n", x_index + xbin / 256.0);
    printf("y index  = %f\n", y_index + ybin / 256.0);
    printf("data[xindex][yindex] = %d\n", value(ptr));
#endif

    /* interpolate using the factional distance between points  */
    if (x_index + 1 >= table->cols)     /* out of bounds on x-axis */
        value1 = value(ptr);    /* use last point in table */
    else
        value1 = interpolate(xbin, value(ptr), value(ptr + (1 << entry_size)));

    if (table->rows == 1)       /* 1D lookup, we are done */
        return value1;

    if (y_index + 1 >= table->rows || ybin == 0)        /* out of bounds (unusual case) */
        return value1;

    {
        /* interpolate using x values from the next row */
        register int value2;

        if (x_index + 1 >= table->cols) /* out of bounds on x-axis */
            value2 = value(ptr + (table->cols << entry_size));
        else
            value2 =
                interpolate(xbin, value(ptr + (table->cols << entry_size)),
                            value(ptr + ((table->cols + 1) << entry_size)));

#ifdef TEST
        printf("after x interpolations, got %d and %d\n", value1, value2);
#endif

        /* interpolate between 2 values */
        return interpolate(ybin, value1, value2);
    }
}                               /* table_lookup_jz() */
